//
// Created by Wirano on 2020/11/13.
//

#include "Drv_OpenMV.h"
#include "Ano_DT.h"
#include "OMV_2013_Parameter.h"
#include <math.h>

//设定
#define OPMV_OFFLINE_TIME_MS  1000  //毫秒

//全局变量
uint16_t offline_check_time;
uint8_t openmv_buf[OMV_BUFFER_LEN];
_openmv_data_st opmv;
_openmv_object_line line[25];
_openmv_object_shape shape[25];

/**********************************************************************************************************
*函 数 名: OpenMV_Byte_Get
*功能说明: OpenMV字节数据获取
*参    数: 字节数据
*返 回 值: 无
**********************************************************************************************************/
void OpenMV_Byte_Get(uint8_t bytedata)
{
    static uint8_t len = 0, rec_pos = 0;

    openmv_buf[rec_pos] = bytedata;

    if (rec_pos == 0) {
        if (bytedata == 0xaa) {
            rec_pos++;
        } else {
            rec_pos = 0;
        }
    } else if (rec_pos == 1) {
        len = bytedata;
        rec_pos++;
    } else if (rec_pos < len + 2) {
        rec_pos++;
    } else if (openmv_buf[rec_pos - 1] == 0x55 && rec_pos == len + 2) {
        OpenMV_Data_Analysis(openmv_buf, len + 2);
        rec_pos = 0;
    } else {
        rec_pos = 0;
    }
}

/**********************************************************************************************************
*函 数 名: OpenMV_Data_Analysis
*功能说明: OpenMV数据解析
*参    数: 缓存数据（形参），长度
*返 回 值: 无
**********************************************************************************************************/
#include "Ano_DT.h"

static void OpenMV_Data_Analysis(uint8_t *buf_data, uint8_t len)
{
    uint8_t num = 0;

//    ANO_DT_SendString("OMV ANALYSIS");

    if (buf_data[2] == 0x02) {
        opmv.cb.sta = buf_data[3];

        if (opmv.cb.sta) {
            num = buf_data[4];
        } else {
            num = 0;
        }

        for (int i = 0; i < num; ++i) {
            shape[i].shape = buf_data[5 + 6 * i + 0];
            shape[i].center_x = ((((int16_t) buf_data[5 + 6 * i + 1]) << 8u) | buf_data[5 + 6 * i + 2]) + OMV_X_OFFSET;
            shape[i].center_y = ((((int16_t) buf_data[5 + 6 * i + 3]) << 8u) | buf_data[5 + 6 * i + 4]) + OMV_Y_OFFSET;
            shape[i].color = buf_data[5 + 6 * i + 5];

            if (shape[i].shape == OMV_SHAPE_CIRCLE) {
                opmv.cb.color_flag = shape[i].shape;
                opmv.cb.pos_x = shape[i].center_x + OMV_CB_X_OFFSET;
                opmv.cb.pos_y = shape[i].center_y + OMV_CB_Y_OFFSET;
            }
        }

        opmv.mode_sta = 1;

        f.send_omv_ct = 1;

//        ANO_DT_SendString("OMV CB REC");

    } else if (buf_data[2] == 0x01) {
        opmv.lt.sta = buf_data[3];

        if (opmv.lt.sta) {
            num = buf_data[4];
        } else {
            num = 0;
        }

        for (int i = 0; i < num; ++i) {
            line[i].start_x = ((((int16_t) buf_data[5 + 6 * i + 0]) << 8u) | buf_data[5 + 6 * i + 1]) + OMV_X_OFFSET;
            line[i].start_y = ((((int16_t) buf_data[5 + 6 * i + 2]) << 8u) | buf_data[5 + 6 * i + 3]) + OMV_Y_OFFSET;
            line[i].len = buf_data[5 + 6 * i + 4];
            line[i].angle = buf_data[5 + 6 * i + 5];

            if (line[i].angle > 170 || line[i].angle < 20) {
                if (line[i].angle > 90) {
                    opmv.lt.angle = (18 - line[i].angle + OMV_LT_ANGLE_OFFSET);
                } else {
                    opmv.lt.angle = -line[i].angle + OMV_LT_ANGLE_OFFSET;
                }
//                if(opmv.lt.angle > -5 && opmv.lt.angle < 5){
//                    opmv.lt.angle = 0;
//                }
                opmv.lt.deviation = (line[i].start_x + line[i].start_y * tan(line[i].angle / 180.0 * 3.14159)) +
                                    OMV_LT_DEVIATION_OFFSET;
                opmv.lt.p_flag = 0;
            }
        }

        opmv.mode_sta = 2;

        f.send_omv_lt = 1;
    }

    OpenMV_Check_Reset();
}

/**********************************************************************************************************
*函 数 名: OpenMV_Offline_Check
*功能说明: OpenMV掉线检测，用来检测硬件是否在线
*参    数: 时间（毫秒）
*返 回 值: 无
**********************************************************************************************************/
void OpenMV_Offline_Check(uint8_t dT_ms)//理应是周期调度的周期调度
{
    if (offline_check_time < OPMV_OFFLINE_TIME_MS) {
        offline_check_time += dT_ms;
    } else {
        opmv.offline = 1;
        opmv.mode_sta = 0;
    }
}

/**********************************************************************************************************
*函 数 名: OpenMV_Check_Reset
*功能说明: OpenMV掉线检测复位，证明没有掉线
*参    数: 无
*返 回 值: 无
**********************************************************************************************************/
static void OpenMV_Check_Reset()
{
    offline_check_time = 0;
    opmv.offline = 0;
}

